/********************************************************
 * In this code, the manipulator moves based on the button
 * inputs of the joystick. A variable representing the desired peg
 * is compared to the actual peg. Depending on the comparison, the
 * manipulator either moves up or down. Also moved variables desiredPeg
 * and actualPeg into this function.
 * - Robert at 3/2/11.
 * ******************************************************/

#include "WPILib.h"
#include "Robot2489.h"

#define LOWOUTPEG 1
#define LOWINPEG 2
#define FEEDERSTATION 3
#define MIDOUTPEG 4
#define MIDINPEG 5

/*
void Robot2489::ManipUp() {
manipulator->Set(0.5); 
}
void Robot2489::ManipDown() {
manipulator->Set(-0.5);
}
void Robot2489::ManipStop() {
manipulator->Set(0);
}
*/

void Robot2489::Manipulator() 
{
	
	/****************************************
	 *The button truths are assigned to the value of the button.
	 Assign values to the variables based on joystick.
	 ****************************************/
	/*manipvalue = leftStick->GetY();
	lowOutButton = leftStick->GetRawButton(6);
	lowInButton = leftStick->GetRawButton(7);
	feederButton = leftStick->GetTrigger();
	midOutButton = leftStick->GetRawButton(10);
	midInButton = leftStick->GetRawButton(11);
	*/
	/******************************************
	 * The switch values are assigned to the value of the switch.
	 * Assign values to the variables based on switch.
	 * ****************************************/
	onLowOutSwitch = !lowOutSwitch->Get();
	onLowInSwitch = !lowInSwitch->Get();
	onMidOutSwitch = !midOutSwitch->Get();
	onMidInSwitch = !midInSwitch->Get();
	onFeederSwitch = !feederSwitch->Get();

	//show which switch is true on the driverstation.
	screen->PrintfLine(DriverStationLCD::kUser_Line1, "%d %d %d %d %d", onLowOutSwitch, onLowInSwitch, onMidOutSwitch, onMidInSwitch, onFeederSwitch); //"Height = %d", manipActualPeg);
	//screen->PrintfLine(DriverStationLCD::kUser_Line2, "%d",onLowOutSwitch);
		
	screen->UpdateLCD();

	/*
	//Button value setting theoretical position;
	if (lowOutButton) //The desired position is now the low out peg.
		manipDesiredPeg = LOWOUTPEG;
	else if (lowInButton) //The desired position is now the low in peg.
		manipDesiredPeg = LOWINPEG;
	else if (feederButton) //The desired position is now the feeder station.
		manipDesiredPeg = FEEDERSTATION;
	else if (midOutButton) //The desired position is now the middle out peg.
		manipDesiredPeg = MIDOUTPEG;
	else if (midInButton) //The desired position is now the middle out peg.
		manipDesiredPeg = MIDINPEG;
	*/
	
	
	//switch values determining actual position.
	if (onLowOutSwitch) //The actual position is at the low out peg.
		manipActualPeg = LOWOUTPEG;
	else if (onLowInSwitch) //The actual position is at the low in peg.
		manipActualPeg = LOWINPEG;
	else if (onFeederSwitch)//The actual position is at the feeder peg.
		manipActualPeg = FEEDERSTATION;
	else if (onMidOutSwitch) //The actual position is at the mid out peg.
		manipActualPeg = MIDOUTPEG;
	else if ((onMidInSwitch)) //The actual position is at the mid in peg.
		manipActualPeg = MIDINPEG;
	
	/*
	//comparing button input to actual switch.
	if (manipDesiredPeg>manipActualPeg)
	{//if the wanted position is higher than the actual position.
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "up");
		manipDirection = manipActualPeg + 1;//The manipulator is going up.
	}
	else if (manipDesiredPeg<manipActualPeg) 
	{//if the wanted position is lower than the actual position.
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "down");
		manipDirection = manipActualPeg - 1;//The manipulator is going down.
	}
	
	if(manipDesiredPeg == manipActualPeg && manipDirection > manipActualPeg && !(onLowOutSwitch||onLowInSwitch||onFeederSwitch||onMidOutSwitch||onMidInSwitch))
	{//if person wants previous position in middle of going up.
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "down");
		if(onLowOutSwitch)
		{
			screen->PrintfLine(DriverStationLCD::kUser_Line3, "stop");
		}
	}
	else if(manipDesiredPeg == manipActualPeg && manipDirection < manipActualPeg && !(onLowOutSwitch||onLowInSwitch||onFeederSwitch||onMidOutSwitch||onMidInSwitch))
	{//if person wants previous position in middle of going down.
		
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "up");
		if(onMidInSwitch)
		{
			screen->PrintfLine(DriverStationLCD::kUser_Line3, "stop");
		}
		
	}
	
	if(manipDesiredPeg==manipActualPeg && (onLowOutSwitch||onLowInSwitch||onFeederSwitch||onMidOutSwitch||onMidInSwitch))
	{//if the switch in the same level as position is pressed.
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "stop");
	}
	screen->UpdateLCD();
	*/
}

